/**
 * @file app_motor.h
 * @brief DJI系列电机(M2006,M3508,M6020)应用层驱动的接口与定义
 * @date 2025-09-25
 * 
 * @attention 
 * 本驱动提供了详尽的上层调用接口,使用者可通过头文件获取高层控制函数
 * pid参数要在 .c 文件下方 static void motor_pid_data_init(...)函数中调整
 * 
 * @example
 * // ===== 如何使用本驱动库(以3508电机速度控制为例) =====
 * 
 * // 1. 在初始化流程中,对电机进行初始化.
 * // motor_ctrl_init(&M3508_Motor,DJI_M3508,&hcan1,LOW);
 * // ...can初始化...
 * // motor_position_init(&M3508_Motor,LOW);
 * 
 * // 2. 在定时器中断中调用回调函数,例如:
 * 
 *  void HAL_TIM_PeriodElapsedCallback(TIM_HandleTypeDef *htim){
 *      if (htim->Instance == TIM3){
 * 		      speed_ctrl(&M3508_Motor,500,500,500,500,DJI_M3508);
 * 		}
 * 	}
 * 
 * // 3. 在can接收中断中获取电机数据,例如:
 * 
 *  void HAL_CAN_RxFifo0MsgPendingCallback(CAN_HandleTypeDef *hcan){
 * 		uint8_t data[8] = {0};
 * 		CAN_RxHeaderTypeDef rx_header;
 * 		if(hcan == &hcan1){
 * 			HAL_CAN_GetRxMessage(hcan, CAN_RX_FIFO0, &rx_header, data);
 * 			rm_motor_receive_msg(&(M3508_Motor.RM_CAN), &rx_header, data);
 * 		}
 *  }
 * ****************************************************************************
 * @copyright Copyright (c) 2025 BNGU
 * 
 */

#ifndef __APP_MOTOR_H__
#define __APP_MOTOR_H__

#include "main.h"
#include "rm_motor.h"
#include "pid.h"


/*---------------------Public Constants---------------------*/
//控制模式(mode)
#define Speed_Mode 0
#define Position_Mode 1
#define Speed_Position_Mode 2
#define Torque_Mode 3
#define Speed_Torque_Mode 4
#define Position_Torque_Mode 5
#define Speed_Position_Torque_Mode 6
#define Zero_Torque_Mode 7

//电机类型(motor_type)
#define DJI_M2006 8
#define DJI_M3508 9
#define DJI_M6020 10

//控制器(control_type)
#define Nom_Ctrl 11
#define MIT_Ctrl 12

//电机分组(MOTOR_GROPE)
#define LOW 13
#define HIGH 14

//PID参数
#define PID_CAL_RATE 1000
#define PID_MAX 8000


/*---------------------Public Data Structures---------------------*/
typedef struct
{
	int16_t exp_speed;
	uint16_t exp_position;
	float exp_torque;
}ExpTypeDef;

typedef struct
{
    int now_position; 
	int last_position; 
	int this_position;
	int last_target;
	int stay_target;
}motor_positionTypeDef;

typedef struct
{
	rm_motor_groupTypeDef RM_CAN;
	float cmd_buffer[4];
	ExpTypeDef motor_exp[4];//原M3508
	pid_controller motor_s_pid[4];
	pid_controller motor_p_pid[4];
	motor_positionTypeDef motor_position[4]; //原motor_3508_position
}DJI_Motor_ControlTypeDef;

extern DJI_Motor_ControlTypeDef M2006_Motor;
extern DJI_Motor_ControlTypeDef M3508_Motor;
extern DJI_Motor_ControlTypeDef M6020_Motor;

/*******初始化函数(MIT与普通模式二选一)*******/
void motor_ctrl_init(DJI_Motor_ControlTypeDef* motor, uint8_t motor_type,CAN_HandleTypeDef* hcan,uint8_t MOTOR_GROUP);
void motor_MIT_ctrl_init(DJI_Motor_ControlTypeDef* motor, uint8_t motor_type,CAN_HandleTypeDef* hcan,uint8_t MOTOR_GROUP);
void motor_position_init(DJI_Motor_ControlTypeDef* motor,uint8_t MOTOR_GROUP);

 /**
  * @brief 此函数集成了所有普通控制模式,但只能控制单个电机
  * @note 此函数用于常规的速度、位置、力矩混合控制
  * @param motor 指向 DJI_Motor_ControlTypeDef 结构体的指针
  * @param id 电机id (0,1,2,3)
  * @param target_speed 目标速度
  * @param target_position 目标位置
  * @param torque 力矩
  * @param mode 控制模式,具体见头文件的八个宏定义
  * @param motor_type 电机型号 (如 DJI_M3508,DJI_M2006,DJI_M6020)
  */
void motor_3508_ctrl(DJI_Motor_ControlTypeDef* motor, uint16_t id, int16_t target_speed, int target_position, float torque, uint8_t mode, uint8_t motor_type);


/*******单个电机,单个控制模式,使用需手动发送命令*******/
void motor_3508_speed_ctrl(DJI_Motor_ControlTypeDef* motor, uint16_t id, int16_t speed);
void motor_3508_position_ctrl(DJI_Motor_ControlTypeDef* motor, uint16_t id, int16_t position);
void motor_3508_speed_position_ctrl(DJI_Motor_ControlTypeDef* motor, uint16_t id, int16_t speed,int position);
void motor_torque_ctrl(DJI_Motor_ControlTypeDef* motor, uint16_t id, float force, uint8_t motor_type);
void motor_3508_speed_torque_ctrl(DJI_Motor_ControlTypeDef* motor, uint16_t id, int16_t speed, float force, uint8_t motor_type);
void motor_3508_position_torque_ctrl(DJI_Motor_ControlTypeDef* motor, uint16_t id, int16_t speed, float force, uint8_t motor_type);
void motor_3508_speed_position_torque_ctrl(DJI_Motor_ControlTypeDef* motor, uint16_t id, int16_t speed, int position, float force, uint8_t motor_type);
void motor_3508_zero_torque_ctrl(DJI_Motor_ControlTypeDef* motor, uint16_t id);



/*******一次控制多个电机*******/
void speed_ctrl(DJI_Motor_ControlTypeDef* motor, int16_t speed_1, int16_t speed_2, int16_t speed_3, int16_t speed_4, uint8_t motor_type);
void position_ctrl(DJI_Motor_ControlTypeDef* motor, int16_t position_1, int16_t position_2, int16_t position_3, int16_t position_4, uint8_t motor_type);
void speed_position_ctrl(DJI_Motor_ControlTypeDef* motor,int16_t speed_1, int16_t speed_2, int16_t speed_3, int16_t speed_4,int position_1, int position_2, int position_3, int position_4, uint8_t motor_type);
void torque_ctrl(DJI_Motor_ControlTypeDef* motor, float torque_1, float torque_2, float torque_3, float torque_4, uint8_t motor_type);
void speed_torque_ctrl(DJI_Motor_ControlTypeDef* motor, int16_t speed_1, int16_t speed_2, int16_t speed_3, int16_t speed_4, float torque_1, float torque_2, float torque_3, float torque_4, uint8_t motor_type);
void position_torque_ctrl(DJI_Motor_ControlTypeDef* motor, int16_t position_1, int16_t position_2, int16_t position_3, int16_t position_4, float torque_1, float torque_2, float torque_3, float torque_4, uint8_t motor_type);
void zero_torque_ctrl(DJI_Motor_ControlTypeDef* motor);

/**
 * @brief 注意初始化方式,motor_ctrl_init,motor_MIT_ctrl_init只能二选一
 * 需手动使用发送函数
 * 
 * @example 
 * //MIT_ctrl:
 * //rm_motor_cmd_refresh(&(motor->RM_CAN), motor->cmd_buffer[0], motor->cmd_buffer[1], motor->cmd_buffer[2], motor->cmd_buffer[3]);
 * //rm_motor_send_msg(&(motor->RM_CAN));
 */
void MIT_ctrl(DJI_Motor_ControlTypeDef* motor, uint8_t id, int16_t speed,int16_t position, float torque, uint8_t motor_type);


#endif